/**
 **********************************************************************
 * File name: filter.cpp
 *
 * COPYRIGHT (C) 2018-2020, ZLOS Development Team
 *
 * The license and distribution terms for this file may be found in 
 * the LICENSE file.
 *
 * Author: ChenLong (Add new editor)
 * Email: worldlong@foxmail.com
 **********************************************************************
**/
#include <math.h>
#include "filter.h"
/*
*/
void LPF2::init(float fs, float fc)
{
  _inited = false;
  
  if(fc <= 0 || fs < 2*fc) {return;}
  
  double fr = fs / fc;
  double ohm = tan(3.14159265358979 / fr);
  double c = 1.0 + 2.0 * cos(3.14159265358979 / 4.0) * ohm + ohm * ohm;
  _b0 = ohm * ohm / c;
  _b1 = 2.0 * _b0;
  _b2 = _b0;
  _a1 = 2.0 * (ohm * ohm - 1.0) / c;
  _a2 = (1.0 - 2.0 * cos(3.14159265358979 / 4.0) * ohm + ohm * ohm) / c;
  
  _y0 = 0;
  _y1 = 0;
  _y2 = 0;
  
  _inited = true;
}
/*
*/
float LPF2::apply(float data)
{
  float filterd;
  
  if(!_inited) {return 0;}

  _y0 = data - _y1 * _a1 - _y2 * _a2;
  
  if (isnan(_y0) || isinf(_y0)) 
  {
    // don't allow bad values to propagate via the filter
    _y0 = data;
  }
  filterd = _y0 * _b0 + _y1 * _b1 + _y2 * _b2;

  _y2 = _y1;
  _y1 = _y0;
  
  return filterd;
}



/*
*/
Filter::Filter(int order, double *a, double *b, double *x, double *y) :
  _order(order),
  _a(a),
  _b(b),
  _x(x),
  _y(y)
  {
  }

/************************************************************
a(1)*y(n) = b(1)*x(n) + b(2)*x(n-1) + ... + b(nb+1)*x(n-nb)
                      - a(2)*y(n-1) - ... - a(na+1)*y(n-na)
where a(1) = 1
*************************************************************/
double Filter::apply(double x)
{
  int order_i;
  
  _x[_order] = x;
  _y[_order] = _b[0] * x;
  
  for(int i=_order; i>0; i--)
  {
    order_i = _order - i;
    _y[_order] += _b[i] * _x[order_i] - _a[i] * _y[order_i];
    _x[order_i] = _x[order_i + 1];
    _y[order_i] = _y[order_i + 1];
  }
  
  return _y[_order];
}
/*
*/
float Filter::apply(float x)
{
  return (float)apply((double)x);
}

